package pl.poznan.put.cs.idss.students.kingcart.core.tracker;

import java.awt.Point;

import pl.poznan.put.cs.idss.students.kingcart.core.model.Features;

import com.googlecode.javacpp.Loader;
import com.googlecode.javacv.cpp.opencv_core.CvMat;
import com.googlecode.javacv.cpp.opencv_video.BackgroundSubtractorMOG2;
import com.googlecode.javacv.cpp.opencv_core.*;
import static com.googlecode.javacv.cpp.opencv_core.CV_RGB;
import static com.googlecode.javacv.cpp.opencv_imgproc.CV_ADAPTIVE_THRESH_MEAN_C;
import static com.googlecode.javacv.cpp.opencv_imgproc.CV_RETR_EXTERNAL;
import static com.googlecode.javacv.cpp.opencv_imgproc.CV_CHAIN_APPROX_NONE;
import static com.googlecode.javacv.cpp.opencv_imgproc.CV_THRESH_BINARY;
import static com.googlecode.javacv.cpp.opencv_imgproc.cvAdaptiveThreshold;
import static com.googlecode.javacv.cpp.opencv_core.CV_TERMCRIT_EPS;
import static com.googlecode.javacv.cpp.opencv_core.CV_TERMCRIT_ITER;
import static com.googlecode.javacv.cpp.opencv_core.IPL_DEPTH_32F;
import static com.googlecode.javacv.cpp.opencv_core.IPL_DEPTH_8U;
import static com.googlecode.javacv.cpp.opencv_core.cvCreateImage;
import static com.googlecode.javacv.cpp.opencv_core.cvGetSize;
import static com.googlecode.javacv.cpp.opencv_imgproc.cvErode;
import static com.googlecode.javacv.cpp.opencv_imgproc.cvThreshold;
import static com.googlecode.javacv.cpp.opencv_imgproc.cvDilate;
import static com.googlecode.javacv.cpp.opencv_imgproc.cvCanny;
import static com.googlecode.javacv.cpp.opencv_core.cvLine;
import static com.googlecode.javacv.cpp.opencv_core.cvRect;
import static com.googlecode.javacv.cpp.opencv_core.cvRectangle;
import static com.googlecode.javacv.cpp.opencv_imgproc.cvBoundingRect;
import static com.googlecode.javacv.cpp.opencv_core.cvPoint;
import static com.googlecode.javacv.cpp.opencv_core.cvCircle;
import static com.googlecode.javacv.cpp.opencv_core.cvSize;
import static com.googlecode.javacv.cpp.opencv_core.cvScalar;
import static com.googlecode.javacv.cpp.opencv_core.cvDrawContours;
import static com.googlecode.javacv.cpp.opencv_core.cvTermCriteria;
import static com.googlecode.javacv.cpp.opencv_highgui.cvConvertImage;
import static com.googlecode.javacv.cpp.opencv_highgui.cvShowImage;
import static com.googlecode.javacv.cpp.opencv_highgui.cvWaitKey;
import static com.googlecode.javacv.cpp.opencv_imgproc.cvGoodFeaturesToTrack;
import static com.googlecode.javacv.cpp.opencv_imgproc.cvFindCornerSubPix;
import static com.googlecode.javacv.cpp.opencv_imgproc.cvFindContours;
import static com.googlecode.javacv.cpp.opencv_video.cvCalcOpticalFlowPyrLK;
import com.googlecode.javacv.cpp.opencv_core.CvPoint;
import com.googlecode.javacv.cpp.opencv_core.CvPoint2D32f;
import com.googlecode.javacv.cpp.opencv_core.CvScalar;
import com.googlecode.javacv.cpp.opencv_core.CvSize;
import com.googlecode.javacv.cpp.opencv_core.CvTermCriteria;
import com.googlecode.javacv.cpp.opencv_core.IplImage;
import com.googlecode.javacv.cpp.opencv_legacy.CvDrawShape;

public class TrackerWithConstantBackground implements Tracker {
	
	IplImage frame;
	IplImage back;
	IplImage fore;
	BackgroundSubtractorMOG2 bg = new BackgroundSubtractorMOG2(30, 16, false);

	CvSeq contour = new CvSeq(null);

	/** sequent used as pointer to contour */
	CvSeq ptr = new CvSeq();

	/** memory to store sequents */
	CvMemStorage storage = CvMemStorage.create();
	private Features features;

	public IplImage track(IplImage currFrame) {

		if (fore == null) {
			fore = IplImage.create(currFrame.width(), currFrame.height(),
					IPL_DEPTH_8U, 1);
		}

		this.bg.apply(currFrame, fore, 0.5);
		// bg.getBackgroundImage(back);
		cvErode(fore, fore, null, 1);
		cvDilate(fore, fore, null, 3);
		// cvShowImage("fore", fore);
		// cvWaitKey(10);
		// cvAdaptiveThreshold(fore, fore, 40,CV_ADAPTIVE_THRESH_MEAN_C,
		// CV_THRESH_BINARY,101,4 );
		// cvThreshold(fore, fore, 1 /* threshold */ , 170 /* max value */ ,
		// CV_THRESH_BINARY);
		 //cvCanny( fore, fore, 4,4, 5 );
		cvFindContours(fore, storage, contour, Loader.sizeof(CvContour.class),
				CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);
		// ==

		CvRect bestBoundBox = null;
//ocenienie podobienstw poszczegolnych konturow
		double mark = Integer.MIN_VALUE;
		if (!contour.isNull())
			for (ptr = contour; ptr != null; ptr = ptr.h_next()) {
				// get the bounding Box surrounding the contour area
				CvRect boundbox = cvBoundingRect(ptr, 0);
				//TODO: byc moze odkomentowac
				//double rectDist = features.rectDist(boundbox);
				double currMark = 0;
				currMark = features.similarity(currFrame, boundbox);
				if (currMark > mark) {
					mark = currMark;
					bestBoundBox = boundbox;
				}
			}
		
		//sprawdzenie, czy cos przekracza prog veta
		if(bestBoundBox == null || features.isVeto(bestBoundBox))
			bestBoundBox = features.getLastBoundBox();
			
			features.setLastBoundBox(bestBoundBox);
			cvRectangle(
					currFrame,
					cvPoint(bestBoundBox.x(), bestBoundBox.y()),
					cvPoint(bestBoundBox.x() + bestBoundBox.width(),
							bestBoundBox.y() + bestBoundBox.height()),
					CV_RGB(0, 255, 0), 2, 8, 0);
			features.setCurrentLocation(new Point(bestBoundBox.x()
					+ bestBoundBox.width() / 2, bestBoundBox.y()
					+ bestBoundBox.height() / 2));
		
		// ==
		cvDrawContours(currFrame, contour, cvScalar(0, 200, 200, 0.5),
				cvScalar(30, 30, 30, 0.5), 1, 1, 1);
		return currFrame;
	}

	public void setFeatures(Features features) {
		this.features = features;

	}

}
